/** Statemachine system
 *
 * This file contains all function used to maintain a statemachine.
 * A statemachine consists of a set of different states. It can be in only one
 * state at one time. A state is defined through an ID (unsigned int), a
 * function to execute on entering the state, a function to execute while in
 * the state and a function to execute when the statemachine leaves the state.
*/
//##############################################################################




#include "LR_statemachine.h"
//##############################################################################




/** Initialise a statemachine.
 *
 * Setup a statemachine structure. Therefore four maps / arrays of identical
 * size are are needed.
 *
 * \parm m The statemachine
 * \parm size The size of the following arrays.
 * \parm enter_map Array of size size for the enter functions.
 * \parm leave_map Array of size size for the leave functions.
 * \parm in_map Array of size size for the in state functions.
 * \parm id_map Array of size size to hold state IDs.
*/
void LR_statemachine_init( LR_statemachine * m,
                           unsigned int size,
                           LR_statemachine_enter_function * enter_map,
                           LR_statemachine_leave_function * leave_map,
                           LR_statemachine_in_function * in_map,
                           LR_statemachine_state * id_map
                         )
{
  unsigned int iterator;
  
  m->size = size;
  // Indicator for a free slot, will be adjusted automatically
  m->no_state = 1000;
  m->id_map = id_map;
  m->enter_map = enter_map;
  m->leave_map = leave_map;
  m->in_map = in_map;
  m->running = -1;
  // Mark every slot as free.
  for (iterator = 0; iterator < m->size; iterator++)
  {
    m->id_map[iterator] = m->no_state;
    m->enter_map[iterator] = 0;
    m->leave_map[iterator] = 0;
    m->in_map[iterator] = 0;
  }
}
//------------------------------------------------------------------------------




/** Add a state to a statemachine.
 *
 * Add a state with corresponding functions to an (initialised) statemachine.
 * Therefore, a free slot is searched (free slots are filled with no_state
 * values). If the new state uses the same (or higher) ID like the current
 * no_state, then use the next higher value (new state ID + 1) and replace old
 * no_state slots with the new no_state indicator.
 *
 * \parm m The statemachine
 * \parm s Indicator for the new state
 * \parm e Entry function
 * \parm l Leave function
 * \parm i In-State function
*/
void LR_statemachine_add( LR_statemachine * m, LR_statemachine_state s,
                          LR_statemachine_enter_function e,
                          LR_statemachine_leave_function l,
                          LR_statemachine_in_function i
                        )
{
  // The place where the new state will be placed into the maps
  signed int place = -1;
  // Iterator used at various opportunities to iterator over the maps
  unsigned int iterator = 0;
  // New state indicating that there is no state
  LR_statemachine_state new_no_state = 0;
  
  // Find a place for the new state
  for (; iterator < m->size; iterator++)
  {
    if (m->id_map[iterator] == m->no_state)
    {
      place = iterator;
      break;
    }
  }
  // Insert the new state at the free place
  if (place != -1)
  {
    // Do we need a new no state indicator?
    if (s >= m->no_state)
    {
      new_no_state = s + 1;
      // Replace old no_state with new_no_state
      for (iterator = 0; iterator < m->size; iterator++)
      {
        if (m->id_map[iterator] == m->no_state)
        {
          m->id_map[iterator] = new_no_state;
        }
      }
      m->no_state = new_no_state;
    }
    m->id_map[place] = s;
    m->enter_map[place] = e;
    m->leave_map[place] = l;
    m->in_map[place] = i;
  }
  else
  {
    // One could catch that error here, e.g. printing "No free slot for new
    // state" or something like that.
  }
}
//------------------------------------------------------------------------------




/** Start a (initialised) statemachine.
 *
 * This function should be used to finally start a statemachine. The start
 * state must be given as parameter. The enter function of that state get's
 * called before going to the in state function.
 * Switching a state is done via the return parameter of the in state function.
 * If the returned state is different from the current state, the current states
 * leave function gets called. After that, the next state is searched via the
 * id_map. If found, the enter function gets called.
 *
 * \parm m The statemachine to start.
 * \parm start Start state (ID).
 *
 * \return LR_STATEMACHINE_STOPPED on normal termination, LR_STATEMACHINE_KILLED
 * on error.
*/
int LR_statemachine_start( LR_statemachine * m, LR_statemachine_state start )
{
  // Used as index of important states
  signed int place = -1;
  // Used to iterator over the id_map to search for states
  unsigned int iterator;
  // The current state
  LR_statemachine_state cur;
  // The next state
  LR_statemachine_state next;
  // The previous state
  LR_statemachine_state prev;

  // find start state
  for (iterator = 0; iterator < m->size; iterator++)
  {
    if (m->id_map[iterator] == start)
    {
      place = iterator;
      break;
    }
  }
  // If we found a start state AND the statemachine is not yet running
  if ((m->running != LR_STATEMACHINE_RUNNING) && (place != -1))
  {
    // indicate that we are running
    m->running = LR_STATEMACHINE_RUNNING;
    // the current state is the start state
    cur = start; //m->id_map[place];
    prev = m->no_state;
    next = cur;
    while ( m->running == LR_STATEMACHINE_RUNNING )
    {
      // call enter function, functions at address 0 would rise problems...
      if (m->enter_map[place] != 0)
      {
        m->enter_map[place]( m, prev, cur );
      }
      // call in function while no change, function at address 0 would rise
      // problems ...
      if (m->in_map[place] != 0)
      {
        while ((m->running == LR_STATEMACHINE_RUNNING) && (next == cur));
        {
          next = m->in_map[place]( m, prev, cur );
        }
      }
      // call leave function, function at address 0 would rise problems ...
      if (m->leave_map[place] != 0)
      {
        m->leave_map[place]( m, prev, cur, next );
      }
      // Search for the next state in the id_map
      place = -1;
      for (iterator = 0; iterator < m->size; iterator++)
      {
        if (m->id_map[iterator] == next)
        {
          place = iterator;
          break;
        }
      }
      if (place != -1)
      {
        // We found the next state.
        prev = cur;
        cur = next;
      }
      else if (m->running == LR_STATEMACHINE_RUNNING)
      {
        // We could not find the next state, because it is not listed in id_map.
        // This is fatal, so terminate the state machine.
        m->running = LR_STATEMACHINE_KILLED;
      }
    }
  }
  return m->running;
}
//------------------------------------------------------------------------------




/** Stop the statemachine.
 *
 * This is a simple wrapper to stop the statemachine from within. Note that it
 * is not protected against concurrent access (e.g. through an interrupt)!
 *
 * The normal order of execution is keept: first entry function, then the in
 * state function and finally the leave function. Stopping the statemachine does
 * not change that!
 *
 * \parm m The statemachine
*/
void LR_statemachine_stop( LR_statemachine * m )
{
  m->running = LR_STATEMACHINE_STOPPED;
}
//------------------------------------------------------------------------------




/** Kill the statemachine.
 *
 * \see LR_statemachine_stop
 * Kills the statemachine. Normal order of execution remains!
 *
 * \parm m The statemachine
*/
void LR_statemachine_kill( LR_statemachine * m )
{
  m->running = LR_STATEMACHINE_KILLED;
}
//------------------------------------------------------------------------------





